#include <joyos.h>

#include "define.h"
#include "led.h"

/**
 * cali<xxx>Led calibrates the threshold of every led in <xxx> group
 * 
 * @requires none
 * @effect modify threshold_led_<yyy>
 * @modifies threshold_led_<yyy> where <yyy> be led names
 */
void caliUnderLed(void){
	printf ("\nPlace FR, FL, CTR Led on BLACK");
	go_click();
	uint16_t black_led_ufr = analog_read(LED_UNDER_FRONTRIGHT);
	uint16_t black_led_ufl = analog_read(LED_UNDER_FRONTLEFT);
	uint16_t black_led_ufc = analog_read(LED_UNDER_CENTER);
	
	printf ("\nPlace FR, FL, CTR Led on WHITE");
	go_click();
	uint16_t white_led_ufr = analog_read(LED_UNDER_FRONTRIGHT);
	uint16_t white_led_ufl = analog_read(LED_UNDER_FRONTLEFT);
	uint16_t white_led_ufc = analog_read(LED_UNDER_CENTER);
	
	printf("\nPlace BR Led on BLACK");
	go_click();
	uint16_t black_led_ubr = analog_read(LED_UNDER_BACKRIGHT);
	
	printf("\nPlace BR Led on WHITE");
	go_click();
	uint16_t white_led_ubr = analog_read(LED_UNDER_BACKRIGHT);

	threshold_led[0] = 0.5*(black_led_ufr + white_led_ufr);
	threshold_led[1] = 0.5*(black_led_ufl + white_led_ufl);
	threshold_led[2] = 0.5*(black_led_ubr + white_led_ubr);
	threshold_led[3] = 0.5*(black_led_ufc + white_led_ufc);
		
	printf("\ntrsh: [%d, %d, %d, %d]", threshold_led[0], threshold_led[1], threshold_led[2], threshold_led[3]);
}

void caliLeftLed(void){
	printf ("\nLeft Near Hole");
	go_click();
	uint16_t black_led_lf = analog_read(LED_LEFT_FRONT);
	uint16_t black_led_lb = analog_read(LED_LEFT_BACK);

	printf ("\nLeft Near Wall");
	go_click();
	uint16_t white_led_lf = analog_read(LED_LEFT_FRONT);
	uint16_t white_led_lb = analog_read(LED_LEFT_BACK);
	
	threshold_led[4] = 0.5*black_led_lf + 0.5*white_led_lf; //Can move a bit far from wall
	threshold_led[5] = 0.5*black_led_lb + 0.5*white_led_lb;
}

void caliRightLed(void){
	printf ("\nRight Near Hole!");
	go_click();
	uint16_t black_led_rf = analog_read(LED_RIGHT_FRONT);
	uint16_t black_led_rb = analog_read(LED_RIGHT_BACK);

	printf ("\nRight New War!");
	go_click();
	uint16_t white_led_rf = analog_read(LED_RIGHT_FRONT);
	uint16_t white_led_rb = analog_read(LED_RIGHT_BACK);
	
	threshold_led[6] = 0.8*black_led_rf + 0.2*white_led_rf;
	threshold_led[7] = 0.8*black_led_rb + 0.2*white_led_rb;
	printf("\nR H: %d W:%d", threshold_led[6],threshold_led[7]);
}

/**
 * portToLed takes port number as input and returns led attached to that port
 * 
 * @requires valid ledPort
 * @effect return led number according to input port
 * @modifies none
 */
uint8_t portToLed(uint8_t ledPort){
	uint8_t led_no;
	
	switch(ledPort){
		case LED_UNDER_FRONTRIGHT:
			led_no = 0;
			break;
		
		case LED_UNDER_FRONTLEFT:
			led_no = 1;
			break;
			
		case LED_UNDER_BACKRIGHT:
			led_no = 2;
			break;

		case LED_UNDER_CENTER:
			led_no = 3;
			break;
				
		case LED_LEFT_FRONT:
			led_no = 4;
			break;
				
		case LED_LEFT_BACK:
			led_no = 5;
			break;
				
		case LED_RIGHT_FRONT:
			led_no = 6;
			break;
				
		case LED_RIGHT_BACK:
			led_no = 7;
			break;
			
		default:
			printf("No LED in this port");
			led_no = 0;
	}
	
	return led_no;
	
}

/**
 * isBlack check color of the surface in front of the led
 * 
 * @requires valid ledPort
 * @effect return 1 if the surface is black, return 0 if the surface is white
 * @modifies none
 */
uint8_t isBlack(uint8_t ledPort){
	uint8_t led_no = portToLed(ledPort);
	uint16_t led_val;
	uint8_t count_black = 0;
	uint8_t total_check_time = 10;
	
	while(1){
		for(uint8_t i = 0; i < total_check_time; i++){
			led_val = analog_read(ledPort);
			if (led_val > threshold_led[led_no])
				count_black++;
		}
	
		if (count_black >= 0.8 * total_check_time)
			return 1;
		else if (count_black <= 0.2 * total_check_time)
			return 0;
		else
			count_black = 0;
	}
	
	return 0;
}
